/* This code is inspired by the "Grbl"
 * Copyright (c) 2009-2011 Simen Svale Skogsrud
 * Copyright (c) 2011 Sungeun K. Jeon
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "parse.h"
#include "extruder_t.h"
#include "gcode_control.h"
#include "rs232_interface.h"

#ifdef _WINDOWS
#include "stdafx.h"
#else
#include "global.h"
#endif

#include "gcode.h"


parser_state_t gc;
linesBuffer lbuf;
//double extrudeLength;
double offset[3];
//double moveLength;



void gc_init(void)
{
    memset(&gc, 0, sizeof(gc));
    gc.feed_rate = SM_DEFAULT_FEED_RATE;
    gc.seek_rate = SM_DEFAULT_SEEK_RATE;
    gc.absolute_mode = true;
    gc.extruder_k = 1;
    gc.extruder_on = true;
    gc.sop = 0;
    gc.next_action = NEXT_ACTION_DEFAULT;
}




//将英制单位mil转换到公制单位mm
static double to_millimeters(double value)
{
    return (gc.inches_mode ? (value * MM_PER_INCH) : value);
}



//解析一行Gcode里面的N，M，G命令字
uint8_t cmd_parse(char* letter, double* value, const char* line, int* char_counter)  //(g_cmd_t *g_cmd)
{
    while (next_statement(letter, value, line, char_counter))
    {
        DBG_TRACE("lettercmd:%c/value:%f/line:%d/char_counter:%d/", *letter, *value, *line, *char_counter);
        int int_value = (int)* value;
        DBG_TRACE("int_value:%d", int_value);
        switch (*letter)  //第一个有效字母
        {
        case 'N':
            break;
        case 'G':
            switch (int_value)
            {
            case 0:         // Rapid motion [快速定位]
                gc.next_action = NEXT_ACTION_SEEK_G0;
                break;
            case 1:         // Coordinated motion ("Straight feed")[直线插补]
                gc.next_action = NEXT_ACTION_LINEAR_G1;
                break;
            case 2:         // 逆圆插补
                gc.next_action = NEXT_ACTION_CW_ARC;
                break;           //Coordinated helical motion ("Arc feed") CW or CCW[圆弧插补]
            case 3:         // 顺圆插补
                gc.next_action = NEXT_ACTION_CCW_ARC;
                break;
            case 4:         // 停顿
                gc.next_action = NEXT_ACTION_DWELL_G4;
                break;
            case 20:        // 使用英寸作为单位
                gc.inches_mode = true;
                break;
            case 21:        // 使用毫米作为单位
                gc.inches_mode = false;
                break;
            case 30:        // 回机床第2、3、4 参考点
            case 28:        // 自动返回机床零点
                gc.next_action = NEXT_ACTION_GO_HOME_G28;
                break;
            case 90:        // 绝对值模式
                gc.absolute_mode = true;
                break;
            case 91:        // 增量模式
                gc.absolute_mode = false;
                break;
            case 92:
                gc.next_action = NEXT_ACTION_RESET_XYZ_G92;
                break;
            case 64:
            case 40:
            case 17:    // G17 Select XY plane
            case 94:    // Feedrate per minute 每分钟给进率
            case 98:    // Feedrate per minute (group type A)
            case 97:    // Constant spindle speed M T Takes an S address integer, which is interpreted as rev/min (rpm). The default speed mode per system parameter if no mode is programmed.
            case 49:    // Tool length offset compensation cancel
            case 80:    // Cancel canned cycle
                break;
            default:
                DBG_ERROR("G%d is not support", int_value);
                FAIL(GCSTATUS_UNSUPPORTED_STATEMENT);
            }
            break;
        case 'M':
            switch (int_value)
            {
            case 0:     // 停止
            case 1:     // 睡眠
            case 112:   // Emergency Stop
                gc.next_action = NEXT_ACTION_STOP;
                break;
            case 3:     // 主轴顺时针转动/激光打开
                gc.spindle_on = 1;
                gc.laser_on = 1;
                gc.spindle_direction = 0;
                break;
            case 4:     // 主轴逆时针转动/激光打开
                gc.spindle_on = 1;
                gc.laser_on = 1;
                gc.spindle_direction = 1;
                break;
            case 5:     // 主轴/激光关闭
                gc.spindle_on = 0;
                gc.laser_on = 0;
                break;
            case 8:     // Coolant on
                gc.coolant = 1;
                break;
            case 9:     // Coolant off
                gc.coolant = 0;
                break;
            case 10:    // 吸尘打开
                gc.vacuum_on = 1;
                break;
            case 11:    // 吸尘关闭
                gc.vacuum_on = 0;
                break;
            case 80:    // 电源打开
                gc.power_on = 1;
                break;
            case 81:    // 电源关闭
                gc.power_on = 0;
                break;
            case 82:    // 挤出机使用绝对坐标模式
                gc.extruder_absolute_mode = 1;
                break;
            case 83:    // 挤出机使用相对坐标模式
                gc.extruder_absolute_mode = 0;
                break;
            case 84:    // Stepers Off
                break;
            case 101:   //  M101 Turn extruder 1 on Forward
                gc.next_action = NEXT_ACTION_EXTRUDER_ON;
                break;
            case 103:   //  M103 Turn all extruders off
                gc.next_action = NEXT_ACTION_EXTRUDER_STOP;
                break;
            case 104:   // M104: Set Extruder Temperature (Fast) Example: M104 S190 Set the temperature of the current extruder to 90oC
                gc.next_action = NEXT_ACTION_EXTRUDER_FAST_T;
                break;
            case 109:   // Set Extruder Temperature Example: M109 S190
                gc.next_action = NEXT_ACTION_EXTRUDER_WAIT_T;
                break;
            case 500:   // 保存设置
                break;
            case 102:   // M102 Turn extruder 1 on Reverse
            case 20:    // 列举SD卡文件
            case 21:    // 初始化SD卡
            case 22:    // 卸载SD卡
            case 23:    // 选择SD卡文件
            case 24:    // 开始/继续SD卡打印
            case 25:    // 停止SD卡打印
            case 30:    // 删除SD卡文件
            case 31:    // 打印时间
            case 52:    // Unload Last tool from spindle
            case 49:    // Feedrate override NOT allowed
            case 48:    // Feedrate override allowed

            case 105:   // M105: Get Extruder Temperature Example: M105 Request the temperature of the current extruder and the build base in degrees Celsius. The temperatures are returned to the host computer. For example, the line sent to the host in response to this command looks like
            case 106:   // M106: Fan On Example: M106 S127 Turn on the cooling fan at half speed. Optional parameter 'S' declares the PWM value (0-255)
            case 107:   // Fan Off
            case 108:   // M108: Set Extruder Speed  Sets speed of extruder motor. (Deprecated in current firmware, see M113)
            case 110:   // Set Current Line Number
            case 113:   // Set Extruder PWM
            case 140:   // Bed Temperature (Fast) Example: M140 S55 Set the temperature of the build bed to 55oC
            case 141:   // Chamber Temperature (Fast) Example: M141 S30 Set the temperature of the chamber to 30oC
            case 142:   // Holding Pressure Example: M142 S1 Set the holding pressure of the bed to 1 bar.
            case 117:
            case 190:
            case 6:
                break;
            default:
                DBG_ERROR("M%d is not support", int_value);
                FAIL(GCSTATUS_UNSUPPORTED_STATEMENT);
            }
            break;
        }
    }
    return (gc.status_code);
}

//解析一行Gcode里面的E，F，P，S，X，Y，Z，I，G，K，R参数
uint8_t para_parse(char* letter, double* value, const char* line, int* char_counter)
{
    while (next_statement(letter, value, line, char_counter))
    {
        double unit_millimeters_value = to_millimeters(*value);
        DBG_TRACE("letterpara:%c/value:%f/line:%d/char_counter:%d", *letter, *value, *line, *char_counter);
        switch (*letter)
        {
        case 'E':
            gc.e_value = (double)* value;
            break;
        case 'F':       //(mm/min)
            if (gc.next_action == NEXT_ACTION_SEEK_G0)
                gc.seek_rate = unit_millimeters_value;
            else
                gc.feed_rate = unit_millimeters_value; // millimeters pr min
            //  if (unit_millimeters_value > SM_MAX_FEEDRATE)
            //      FAIL(GCSTATUS_UNSOPORTED_FEEDRATE);
            break;
        case 'P':
            gc.pause_value = (int)* value;
            break;
        case 'S':
            gc.s_value = (int16_t)* value;
            break;
        case 'X':
        case 'Y':
        case 'Z':
            if (gc.absolute_mode)
                gc.position[*letter - 'X'] = unit_millimeters_value;
            else
                gc.position[*letter - 'X'] += unit_millimeters_value;
            break;
        case 'I':
        case 'J':
        case 'K':
            offset[*letter - 'I'] = unit_millimeters_value;
            break;
        case 'R':
            gc.radius = unit_millimeters_value;
            gc.radius_mode = true;
            break;
        case 'G':
        case 'N':
        case 'M':
            break;
        default:
            FAIL(GCSTATUS_UNSUPPORTED_PARAM);
        }
    }
    return gc.status_code;
}

//根据解析到的参数和命令字执行相应的控制
uint8_t gcode_control(double* oldPosition, double* dx, double* dy, double* dz, double feed_rate)
{
    switch (gc.next_action)
    {
    case NEXT_ACTION_DWELL_G4:
        mc_dwell(gc.pause_value * 1000);
        break;
    case NEXT_ACTION_RESET_XYZ_G92:
        gc.position[X_AXIS] = gc.position[Y_AXIS] = gc.position[Z_AXIS] = 0;
        break;
    case NEXT_ACTION_STOP:
        mc_end();
        break;
    case NEXT_ACTION_GO_HOME_G28:
    case NEXT_ACTION_SEEK_G0:
        if (!mc_line(gc.position[X_AXIS], gc.position[Y_AXIS], gc.position[Z_AXIS], 0, gc.moveLength, feed_rate))  // mm * 60000/(mm/min) = msec
        {
            return GCSTATUS_CANCELED;
        }
        break;
    case NEXT_ACTION_LINEAR_G1:
        if (!mc_line(gc.position[X_AXIS], gc.position[Y_AXIS], gc.position[Z_AXIS], gc.extruder_length, gc.moveLength, feed_rate))
        {
            return GCSTATUS_CANCELED;
        }
        break;
    case NEXT_ACTION_CW_ARC:
    case NEXT_ACTION_CCW_ARC:
        if (gc.radius_mode)  //半径模式
        {
            double h_x2_div_d;      // tan(OCT)
            /* We need to calculate the center of the circle that has the designated radius and passes
               through both the current position and the target position. This method calculates the following
               set of equations where [x,y] is the vector from current to target position, d == magnitude of
               that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
               the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
               length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
               [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
               计算出设计的半径的圆的中心位置，并且圆弧通过当前和目标位置。
               这种方法计算出邋X,Y在如下方程式中的解。


                 d^2 == x^2 + y^2           d:radius
                 h^2 == r^2 - (d/2)^2
                 i == x/2 - y/d*h
                 j == y/2 + x/d*h

                                  O <- [i,j]
                               -  |
                            -     |
                       r -        |   h
                      -           |
                   -              |
    [0,0] ->   C -----------------+----------------- T  <- [x,y]
                                  | <------ d/2 ---->|

                   C - Current position
                   T - Target position
                   O - center of circle that pass through both C and T
                   d - distance from C to T
                   r - designated radius
                   h - distance from center of CT to O

                   Expanding the equations:

                   d -> sqrt(x^2 + y^2)
                   h -> sqrt(4 * r^2 - x^2 - y^2)/2
                   i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
                   j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2

                   Which can be written:

                   i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
                   j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2

                   Which we for size and speed reasons optimize to:

                   h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
                   i = (x - (y * h_x2_div_d))/2
                   j = (y + (x * h_x2_div_d))/2

                   */
            h_x2_div_d = -sqrt(4 * gc.radius * gc.radius - *dx * *dx - *dy * *dy) / hypot(*dx, *dy); // == -(h * 2 / d)  ，tan(OCT)
            // If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any
            // real CNC, and thus - for practical reasons - we will terminate promptly:
            if (isnan(h_x2_div_d))
            {
                FAIL(GCSTATUS_FLOATING_POINT_ERROR);
            }
            // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
            if (gc.next_action == NEXT_ACTION_CCW_ARC)
            {
                h_x2_div_d = -h_x2_div_d;
            }

            /* The counter clockwise circle lies to the left of the target direction. When offset is positive,
               the left hand circle will be generated - when it is negative the right hand circle is generated.
                                                          T  <-- Target position
                                                          ^
               Clockwise circles with this center         |          Clockwise circles with this center will have
               will have > 180 deg of angular travel      |          < 180 deg of angular travel, which is a good thing!
                                                \         |          /
   center of arc when h_x2_div_d is positive ->  x <----- | -----> x <- center of arc when h_x2_div_d is negative
                                                          |
                                                          |
                                                          C  <-- Current position                                 */
                                                          // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
                                                          // even though it is advised against ever generating such circles in a single line of g-code. By
                                                          // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
                                                          // travel and thus we get the unadvisably long arcs as prescribed.
            if (gc.radius < 0)
            {
                h_x2_div_d = -h_x2_div_d;
                gc.radius = -gc.radius; // Finished with r. Set to positive for mc_arc
            }
            // Complete the operation by calculating the actual center of the arc
            offset[X_AXIS] = 0.5 * (*dx - (*dy * h_x2_div_d));  // 圆心位置
            offset[Y_AXIS] = 0.5 * (*dy + (*dx * h_x2_div_d));
        }
        else
        {
            // Offset mode specific computations
            gc.radius = hypot(offset[X_AXIS], offset[Y_AXIS]); // Compute arc radius for mc_arc
        }
        mc_arc(oldPosition, gc.position, offset, gc.feed_rate, gc.radius, gc.next_action == NEXT_ACTION_CW_ARC);
        break;
    case NEXT_ACTION_EXTRUDER_STOP:
        cnc_extruder_stop();
        gc.extruder_on = false;
        break;
    case NEXT_ACTION_EXTRUDER_ON:
        cnc_extruder_on();
        gc.extruder_on = true;
        break;
    case NEXT_ACTION_EXTRUDER_FAST_T:
        cnc_extruder_t(gc.s_value, false);
        break;
    case NEXT_ACTION_EXTRUDER_WAIT_T:
        cnc_extruder_t(gc.s_value, true);
        break;
    }
    return gc.status_code;
}


// Executes one line of 0-terminated G-Code. The line is assumed to contain only upper case
// characters and signed floating point values (no whitespace).
//执行1行G代码，并且假定这行代码只含有大写的字符和有符号浮点数
uint8_t gc_execute_line(const char* line)
{
    double feed_rate;
    int char_counter = 0;
    char letter;
    double value, oldPosition[3], dx, dy, dz;//, moveLength;//, offset[3];//, radius = 0;
    //    int pause_value = 0;
    //    uint8_t radius_mode = false;

    gc.status_code = GCSTATUS_OK;

    if (line[0] == ';'
        || line[0] == '('
        || line[0] == '%'
        || line[0] == '\n'
        )
    {
        return GCSTATUS_OK; // comments
    }

    //------------- Pass 1: Commands
    cmd_parse(&letter, &value, line, &char_counter);

    //----------------
    if (gc.status_code)
    {
        return(gc.status_code);
    }

    // Pass 2: Parameters
    char_counter = 0;
    oldPosition[X_AXIS] = gc.position[X_AXIS];
    oldPosition[Y_AXIS] = gc.position[Y_AXIS];
    oldPosition[Z_AXIS] = gc.position[Z_AXIS];
    offset[0] = offset[1] = offset[2] = 0;

    gc.e_value = 0;
    para_parse(&letter, &value, line, &char_counter);
    if (gc.status_code)
    {
        return gc.status_code;
    }

    if (gc.next_action == NEXT_ACTION_GO_HOME_G28)
    {
        gc.position[0] = gc.position[1] = gc.position[2] = 0;
    }
    dx = gc.position[X_AXIS] - oldPosition[X_AXIS];
    dy = gc.position[Y_AXIS] - oldPosition[Y_AXIS];
    dz = gc.position[Z_AXIS] - oldPosition[Z_AXIS];

    if (fabs(dx) < SM_TOO_SHORT_SEGMENT_MM)
    {
        dx = 0.0;
        gc.position[X_AXIS] = oldPosition[X_AXIS];
    }
    if (fabs(dy) < SM_TOO_SHORT_SEGMENT_MM)
    {
        dy = 0.0;
        gc.position[Y_AXIS] = oldPosition[Y_AXIS];
    }
    if (fabs(dz) < SM_TOO_SHORT_SEGMENT_MM)
    {
        dz = 0.0;
        gc.position[Z_AXIS] = oldPosition[Z_AXIS];
    }

    gc.moveLength = sqrt(dx * dx + dy * dy + dz * dz);
    feed_rate = (gc.next_action == NEXT_ACTION_SEEK_G0 ? gc.seek_rate : gc.feed_rate);

    if (gc.extruder_on)
    {
        if (gc.e_value == 0.0)
        {
            gc.extruder_length += gc.moveLength * gc.extruder_k;
        }
        else
        {
            gc.extruder_length = gc.e_value;
        }
    }

    if ((gc.next_action == NEXT_ACTION_SEEK_G0 ||
        gc.next_action == NEXT_ACTION_LINEAR_G1 ||
        gc.next_action == NEXT_ACTION_CW_ARC ||
        gc.next_action == NEXT_ACTION_CCW_ARC)
        && gc.moveLength < SM_TOO_SHORT_SEGMENT_MM)
    {
        // too short move.. Ignore
        gc.position[X_AXIS] = oldPosition[X_AXIS];
        gc.position[Y_AXIS] = oldPosition[Y_AXIS];
        gc.position[Z_AXIS] = oldPosition[Z_AXIS];
        return(gc.status_code);
    }

    // Perform any physical actions
    gcode_control(oldPosition, &dx, &dy, &dz, feed_rate);
    return(gc.status_code);
}
